
#ifndef FOC_H
#define FOC_H

#include "sys_init.h"
#include "control.h"
#include "stdint.h"


#define PERIOD 4999
#define OVERMODULATION 0.5f
#define DTC_MAX 0.94f          // Max phase duty cycle
#define DTC_MIN 0.06f          // Min phase duty cycle
#define MATH_TWO_PI               ((float)(6.283185307179586f))
#ifndef PI
#define PI               3.14159265358979f
#endif

typedef struct
{
    float    fInvCurrUReal;
    float    fInvCurrVReal;
    float    fInvCurrWReal;

    float    fInvIalfa;
    float    fInvIbeta;

    float    fTheta;
    float    fInvCostheta;
    float    fInvSintheta;

    float    fInvId;
    float    fInvIq;
    float    fInvIqFilter;
    float    fInvIdFilter;

    float    fInvIqRef;
    float    fInvIqRefFilt;

    float    fInvUd;
    float    fInvUq;
    float    fInvUqLowSpdSet;

    float    fInvUalfa;
    float    fInvUbeta;
    float    Vbus;

    float    Uref;
    float    Vref;
    float    Wref;

    uint16_t   wAInvPwmDuty;
    uint16_t   wBInvPwmDuty;
    uint16_t   wCInvPwmDuty;
} FOC_PARAMETER;
extern FOC_PARAMETER  stFOC;


static inline float MATH_incrAngle(const float angle_rad, const float angleDelta_rad)
{
    float angleNew_rad;

    angleNew_rad = angle_rad + angleDelta_rad;

    angleNew_rad = (angleNew_rad > MATH_TWO_PI)  ? angleNew_rad - MATH_TWO_PI : angleNew_rad;
    angleNew_rad = (angleNew_rad < -MATH_TWO_PI) ? angleNew_rad + MATH_TWO_PI : angleNew_rad;

    return (angleNew_rad);
} // end of MATH_incrAngle() function

void FOCRun(FOC_PARAMETER* FOCpara,float theta);
void runClarke(FOC_PARAMETER *pFOC);
float getOpenAngle(void);

#endif
